﻿using HalconDotNet;
using LD.Host;
using LD.PLC;
using System;
using System.IO;
using YTE.Control;

namespace LD.Vision
{
    public partial class FrmCalibByPLC : FrmBase
    {
        private AbsCamera camera;
        private HHomMat2D mat2d = new HHomMat2D();
        private int[] calib_x = new int[] { -1, 0, 1, -1, 0, 1, -1, 0, 1 };
        private int[] calib_y = new int[] { 1, 1, 1, 0, 0, 0, -1, -1, -1 };
        private int[] calib_r = new int[] { 4, 2, 0, -2, -4 };
        private float errorVal = 0;

        public FrmCalibByPLC()
        {
            InitializeComponent();
            this.Shown += FrmCam_Shown;
        }

        private void FrmCam_Shown(object sender, EventArgs e)
        {
            foreach (var item in CameraFactory.Instance.CameraList)
            {
                this.cob_camera.Items.Add(item.Key);
            }
        }

        private void cob_camera_SelectedIndexChanged(object sender, EventArgs e)
        {
            string cameraName = this.cob_camera.Text;
            if (CameraFactory.Instance.CameraList.ContainsKey(cameraName))
            {
                camera = CameraFactory.Instance.CameraList[cameraName];

                if (camera != null)
                {
                    foreach (var item in camera.ShapeModels)
                    {
                        this.cob_shape.Items.Add(item.Key);
                    }

                    if (this.camera.CalibDatas.ContainsKey(camera.Name))
                    {
                        var calData = this.camera.CalibDatas[camera.Name];
                        this.cob_calType.SelectedIndex = calData.CalType;
                        UiUpdate(calData.Px, calData.Py, calData.Wx, calData.Wy);
                    }
                }
            }
        }

        private void btn_saveImage_Click(object sender, EventArgs e)
        {
            if (camera != null && this.cob_shape.SelectedIndex > -1)
            {
                this.dgv_data.Rows.Clear();
                    PLCAuto();
            }
        }
        private void PLCAuto()
        {
            bool isOver = false;
            //误差值
            errorVal = 0;
            //机械必须移动至中心位置且发送开始信号，才会执行此步骤
            //间隔距离
            int step = CacheHelperVision.GetStruct<int>(CONST.CALIB1_MOVESTEP);
            var isStart = PLCFactory.Instance.ReadInt16(CacheHelperVision.Get<string>(CONST.CALIB1_START));
            if (isStart.IsSuccess && isStart.Content == 1)
            {
                double[] px = new double[14];              //世界坐标x
                double[] py = new double[14];              //世界坐标y
                double[] mx = new double[14];              //像素坐标x
                double[] my = new double[14];              //像素坐标y

                #region 计算PLC运动轨迹

                //获取PLC当前位置
                //起始为原点
                var x = PLCFactory.Instance.ReadFloat(CacheHelperVision.Get<string>(CONST.CALIB1_X));
                var y = PLCFactory.Instance.ReadFloat(CacheHelperVision.Get<string>(CONST.CALIB1_Y));
                var r = PLCFactory.Instance.ReadFloat(CacheHelperVision.Get<string>(CONST.CALIB1_Z));
                //计算运动轨迹坐标
                float[] px9 = new float[9];              //世界坐标x
                float[] py9 = new float[9];              //世界坐标y
                float[] pr5 = new float[5];              //世界坐标x
                for (int i = 0; i < 9; i++)
                {
                    px9[i] = x.Content + calib_x[i] * step;
                    py9[i] = y.Content + calib_y[i] * step;
                }
                for (int i = 0; i < 5; i++)
                {
                    pr5[i] = r.Content + calib_r[i] * step;
                }
                //通知PLC计算好位置了
                PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_POINTOK), 0);
                int len = calib_x.Length;

                #endregion

                #region 9点标定

                //9点标定
                for (int i = 0; i < len; i++)
                {

                    //移动位置，X,Y,Z
                    PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_X_WRITE), px9[i]);
                    PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_Y_WRITE), py9[i]);

                    //通知PLC开始移动
                    ushort flag = 1;
                    PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_MOVESTART), flag);

                    //等待PLC是否移动完成
                    isOver = PLCFactory.Instance.Wait(CacheHelperVision.Get<string>(CONST.CALIB1_MOVEOVER), 1, 3000);
                    if (!isOver)
                        return;                  //超时退出标定

                    if (!FindShape(px9[i], py9[i], out var row, out var column, out var angle, out var score, out var _x, out var _y))
                        return;

                    px[i] = _x;
                    py[i] = _y;
                    mx[i] = row;
                    my[i] = column;

                    //标定完成
                    CalibResult(true);
                    camera.SaveImage(ResultType.None, $"Calibration\\{i + 1}.bmp");
                }

                mat2d.VectorToHomMat2d(new HTuple(mx), new HTuple(my), new HTuple(px), new HTuple(py));

                //移动位置至原点
                PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_X_WRITE), x.Content);
                PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_Y_WRITE), y.Content);
                //等待PLC是否移动完成
                isOver = PLCFactory.Instance.Wait(CacheHelperVision.Get<string>(CONST.CALIB1_MOVEOVER), 1, 3000);
                if (!isOver)
                    return;                  //超时退出标定
                #endregion

                #region 5点标定.
                Circle circle = new Circle();
                if (this.chk_five.Checked)
                {
                    //5点标定
                    len = calib_r.Length;
                    int index = 9;
                    for (int i = 0; i < len; i++)
                    {
                        PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_Y_WRITE), pr5[i]);

                        //等待PLC是否移动完成
                        isOver = PLCFactory.Instance.Wait(CacheHelperVision.Get<string>(CONST.CALIB1_MOVEOVER), 1, 3000);
                        if (!isOver)
                            return;                  //超时退出标定

                        if (!FindShape(px9[i], py9[i], out var row, out var column, out var angle, out var score, out var _x, out var _y))
                            return;

                        mx[i + index] = row;
                        mx[i + index] = column;
                        //标定完成
                        CalibResult(true);
                        camera.SaveImage(ResultType.None, $"Calibration\\{i + 10}.bmp");
                    }
                    //5点数据拟合圆
                    //计算圆心半径
                    for (int i = 0; i < 5; i++)
                    {
                        var qx = mat2d.AffineTransPoint2d((double)mx[i + index], my[i + index], out var qy);
                        px[i + index] = qx;
                        px[i + index] = qy;
                    }
                    //5点数据
                    double[] pxr = new double[5];
                    double[] pyr = new double[5];
                    Array.Copy(px, 9, pxr, 0, 5);
                    Array.Copy(py, 9, pyr, 0, 5);
                    //拟合圆
                    HXLDCont hXLDCont = new HXLDCont();
                    hXLDCont.GenContourPolygonXld(pxr, pyr);

                    //拟合参数
                    string algorithm = "geotukey";
                    int maxNumPoints = -1;
                    double maxClosureDist = 0;
                    int clippingEndPoints = 0;
                    int iterations = 3;
                    double clippingFactor = 2.0;
                    hXLDCont.FitCircleContourXld(algorithm,
                                                 maxNumPoints,
                                                 maxClosureDist,
                                                 clippingEndPoints,
                                                 iterations,
                                                 clippingFactor,
                                                out double row1,
                                                out var column1,
                                                out double radius,
                                                out var startPhi,
                                                out var endPhi,
                                                out var pointOrder
                                                );
                    circle.Row = row1;
                    circle.column1 = column1;
                    circle.Radius = radius;
                    circle.StartPhi = startPhi;
                    circle.EndPhi = endPhi;
                    circle.PointOrder = pointOrder;
                }
                #endregion


                string fileName = camera.Name;
                CalibrationModel calibration = new CalibrationModel();
                calibration.Px = px;
                calibration.Py = py;
                calibration.Wx = mx;
                calibration.Wy = my;
                calibration.Circle = circle;
                calibration.HHomMat2D = mat2d;
                calibration.Name = fileName;
                calibration.CalType = this.cob_calType.SelectedIndex;

                if (camera.CalibDatas.ContainsKey(fileName))
                    camera.CalibDatas[fileName] = calibration;
                else
                    camera.CalibDatas.Add(fileName, calibration);

                //标定完成
                PLCFactory.Instance.Write(CacheHelperVision.Get<string>(CONST.CALIB1_OVER), 0);

                //数据展示在UI
                UiUpdate(px, py, mx, my);
            }
        }
        private void BCAuto()
        {

        }
        private void Calibration()
        {

        }
        private bool FindShape(float x, float y, out HTuple row, out HTuple column, out HTuple angle, out HTuple score, out float px, out float py)
        {
            #region 拍照，获取中心点
            try
            {
                row = 0; column = 0; angle = 0; score = 0; py = 0; px = 0;
                //拍照
                var isSuccess = camera.Trigger();
                if (!isSuccess)
                {
                    Log4.Warr($"【{camera.Name}】拍照失败");
                    CalibResult(false);
                    return false;
                }
                var model = camera.ShapeModels[this.cob_shape.Text];

                //模板匹配
                //求出中心点
                isSuccess = Shape.FindShapeModel(model, FrmVisionMain.Instance.HWindow[camera.Name].Image, out var row1, out var column1, out var angle1, out var score1);
                if (!isSuccess)
                {
                    //匹配失败
                    CalibResult(false);
                    Log4.Warr($"【{camera.Name}】模板匹配失败");
                    camera.SaveImage(ResultType.NG);
                    return false;
                }
                row = row1;
                column = column1;
                angle = angle1;
                score = score1;
                #endregion

                #region 计算机械误差
                var _x = PLCFactory.Instance.ReadFloat(CacheHelperVision.Get<string>(CONST.CALIB1_X));
                var _y = PLCFactory.Instance.ReadFloat(CacheHelperVision.Get<string>(CONST.CALIB1_Y));
                float e = Math.Abs(_x.Content - x);
                //计算误差多少
                if (errorVal < e)
                    errorVal = e;
                e = Math.Abs(_x.Content - y);
                if (errorVal < e)
                    errorVal = e;
                #endregion

                px = _x.Content;
                py = _y.Content;
                return true;
            }
            catch (Exception)
            {
                throw;
            }
        }
        /// <summary>
        /// 标定结果通知PLC
        /// </summary>
        /// <param name="isSuccess">成功否</param>
        private void CalibResult(bool isSuccess)
        {
            ushort flag = 1;
            string address = "";
            if (!isSuccess)
            {
                flag = 2;
                address = CacheHelperVision.Get<string>(CONST.CALIB1_NG);
            }
            else
            {
                address = CacheHelperVision.Get<string>(CONST.CALIB1_OK);
            }
            PLCFactory.Instance.Write(address, flag);
        }
        private void UiUpdate(double[] px, double[] py, double[] mx, double[] my)
        {
            int len = px.Length;

            for (int i = 0; i < len; i++)
            {
                this.dgv_data.Rows.Add(i + 1, px[i], py[i], mx[i], my[i]);
            }
        }

        private void btn_save_Click(object sender, EventArgs e)
        {
            //保存
            string path = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $@"Vision\{camera.Name}\Calibration");
            foreach (var item in camera.CalibDatas)
            {
                JsonSerializeHelper.Save(item.Value, Path.Combine(path, item.Key + ".ldc"));
                //SerializeHelper.Save(item.Value, Path.Combine(path, item.Key + ".ldc"));
            }
        }

        private void chk_five_CheckedChanged(object sender, EventArgs e)
        {
            if (this.chk_five.Checked)
            {
                this.chk_night.Checked = true;
            }
        }
    }
}
